#include "WPILib.h"

#ifndef __ROBOT2489_h__
#define __ROBOT2489_h__

#define KICKERREADYINGTIME 0.4 //time it takes for the kicker to charge OR relax OR anti-charge, should be very small
#define LATCHOPENTIME .4 //time it takes for latch to open AND kick to actually occur.
#define KICKERRETRACTTIME 0.8 //time it takes for kicker piston to retract
#define LATCHCLOSETIME 0.2 //time it takes for latch to close so kicking will be ready again.
#define FORWARDDRIVETIME 1 //autonomous forward drive increments
class Robot2489: public SimpleRobot
{
	Jaguar *leftFrontMotorController;
	Jaguar *leftBackMotorController;
	Jaguar *rightFrontMotorController;
	Jaguar *rightBackMotorController;
	RobotDrive *myRobot; // robot drive system
	Joystick *rightStick; // joystick 1 (arcade stick or right tank stick)
	Joystick *leftStick; // joystick 2 (tank left stick)
	Joystick *manipStick;
//	DriverStation *ds; // driver station object
	Compressor *KarlKompressor; // this is a compressor
	Solenoid *solKickOut; // this is the left solenoid
	Solenoid *solKickIn; // this is the right solenoid
	Solenoid *solLatch; //this is the latch solenoid
	Timer *kickTimar;
//	DriverStationLCD *screen;
//	DriverStationEnhancedIO *dseio;
	
	PIDController LeftBackPIDController;
	PIDController LeftFrontPIDController;
	PIDController RightBackPIDController;
	PIDController RightFrontPIDController;


	bool kickTriggered;

	enum // Driver Station jumpers to control program operation
	{
		ARCADE_MODE = 1, // Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
		ENABLE_AUTONOMOUS = 2,
	// Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
	} jumpers;

public:
	Robot2489();
	~Robot2489();
	void Autonomous();
	void OperatorControl();
	void kickFunction();
	void ChargeKicker();
	void RetractKicker();
	void OpenLatch();
	void CloseLatch();
	void RelaxKicker();
	double MotorControl(double);
	void DriveForward(float, float);
	void DriveBackward(float, float);
	void TurnRight();
};

#endif
